#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_ROTATIONAL_SCAN_MATCHER_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_ROTATIONAL_SCAN_MATCHER_H_

#include <vector>
#include "Eigen/Geometry"
#include "cartographer/sensor/point_cloud.h"

namespace cartographer
{
    namespace mapping
    {
        namespace scan_matching
        {
            /*
            * 以函数ConstraintBuilder3D::DispatchScanMatcherConstruction中的使用为例来讲解该类的作用;
            * 在构造该类对象时传入子图的角度直方图，在调用成员函数Match时传入的是点云的角度直方图;
            * 函数Match的作用就是匹配子图和点云的角度直方图;
            * 
            * 在构造三维点云的角度直方图时，在Z轴上对点云进行了分片，然后计算每个分片点云的质心，计算某点和其参考点到
            * 质心的夹角(参考点一般是点云的起始点),然后将夹角放入直方图中，同时也计算了一个权重，直方图存的就是这个权重；
            * 在暴力搜搜索的时候饶Z轴采样了多个yaw角度，此时就是调用RotateHistogram计算每个yaw对应的直方图;
            */
            class RotationalScanMatcher
            {
            public:
                // Rotates the given 'histogram' by the given 'angle'. This might lead to
                // rotations of a fractional bucket which is handled by linearly
                // interpolating.
                static Eigen::VectorXf RotateHistogram(const Eigen::VectorXf &histogram, float angle);

                // Computes the histogram for a gravity aligned 'point_cloud'.
                static Eigen::VectorXf ComputeHistogram(const sensor::PointCloud &point_cloud, int histogram_size);

                explicit RotationalScanMatcher(const Eigen::VectorXf *histogram);

                // Scores how well 'histogram' rotated by 'initial_angle' can be understood as
                // further rotated by certain 'angles' relative to the 'nodes'. Each angle
                // results in a score between 0 (worst) and 1 (best).
                std::vector<float> Match(const Eigen::VectorXf &histogram, float initial_angle, const std::vector<float> &angles) const;

            private:
                const Eigen::VectorXf *histogram_;
            };
        } // namespace scan_matching
    }     // namespace mapping
} // namespace cartographer

#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_ROTATIONAL_SCAN_MATCHER_H_
